import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.actions import Node


def generate_launch_description():

    share_dir = get_package_share_directory('lio_sam_base')
    parameter_file = LaunchConfiguration('params_file')
    xacro_path = os.path.join(share_dir, 'config', 'robot.urdf.xacro')
    rviz_config_file = os.path.join(share_dir, 'config', 'rviz2.rviz')

    # --- 1. 添加 use_sim_time 的启动参数声明 ---
    # 这允许我们从命令行传递 use_sim_time:=true 或 use_sim_time:=false
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true'
    )
    # 将启动参数的值赋给一个变量，以便后续使用
    use_sim_time = LaunchConfiguration('use_sim_time')

    params_declare = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(
            share_dir, 'config', 'params.yaml'),
        description='Path to the ROS2 parameters file to use.')

    print("urdf_file_name : {}".format(xacro_path))

    return LaunchDescription([
        # --- 2. 将 use_sim_time 参数声明加入到LaunchDescription中 ---
        use_sim_time_arg,
        params_declare,

        # ==================================================================
        # =========  在这里添加我们的 IMU 过滤器节点 ===============
        # ==================================================================
        Node(
            package='imu_filter',
            executable='imu_filter_node',
            name='imu_filter_node',
            output='screen',
            # --- 3. 关键！将use_sim_time参数传递给这个节点 ---
            parameters=[{'use_sim_time': use_sim_time}]
        ),
        # ==================================================================

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments='0.0 0.0 0.0 0.0 0.0 0.0 map odom'.split(' '),
            # 将 use_sim_time 也传递给 tf2 节点
            parameters=[parameter_file, {'use_sim_time': use_sim_time}],
            output='screen'
        ),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{
                'robot_description': ParameterValue(Command(['xacro ', xacro_path]), value_type=str),
                # 将 use_sim_time 也传递给 robot_state_publisher
                'use_sim_time': use_sim_time
            }]
        ),
        Node(
            package='lio_sam_base',
            executable='lio_sam_base_imuPreintegration',
            name='lio_sam_imuPreintegration',
            parameters=[parameter_file, {'use_sim_time': use_sim_time}],
            output='screen'
        ),
        Node(
            package='lio_sam_base',
            executable='lio_sam_base_imageProjection',
            name='lio_sam_imageProjection',
            parameters=[parameter_file, {'use_sim_time': use_sim_time}],
            output='screen'
        ),
        Node(
            package='lio_sam_base',
            executable='lio_sam_base_featureExtraction',
            name='lio_sam_featureExtraction',
            parameters=[parameter_file, {'use_sim_time': use_sim_time}],
            output='screen'
        ),
        Node(
            package='lio_sam_base',
            executable='lio_sam_base_mapOptimization',
            name='lio_sam_mapOptimization',
            parameters=[parameter_file, {'use_sim_time': use_sim_time}],
            output='screen'
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_file],
            # 将 use_sim_time 也传递给 Rviz2
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'
        )
    ])